
#ifndef __RBFFUNCTION_3TO3_H__
#define __RBFFUNCTION_3TO3_H__

#include "rbfFunction_3to1.h"

class RbfFunction3to3 : public RbfFunction3to1 {
   
public:
   /// Default constructor
   RbfFunction3to3():
      RbfFunction3to1() {
      npolyterms = 4;
      //npolyterms = 10;
   }
   /// Destructor
   virtual ~RbfFunction3to3() {
      
   }
   
   void setCentersAndWeights(vector<Point3> &c, vector<Point3> &w) {
      centers = c;
      weights = w;
   }
   
   /// Performs the product: w = iM*fcenters
   /// @param iM the RBF inverse matrix
   /// @param cntrs the centers array
   /// @param w the weights array
   void genWeights(vector<Point3> &fcenters) {
      
      assert(centers.size() == fcenters.size());
      unsigned int n = fcenters.size();
      assert(npolyterms == iM.m-n);
      Matrix RightSide(iM.m, 3);
      for (unsigned int i=0; i<n; ++i) {
         const Point3 &Q = fcenters[i];
         RightSide(i,0)=Q[0];
         RightSide(i,1)=Q[1];
         RightSide(i,2)=Q[2];
      }
      
      /// additional restrictions initialization
      for (unsigned int i=0; i<npolyterms; ++i) {
         RightSide(n+i,0)=0.0;
         RightSide(n+i,1)=0.0;
         RightSide(n+i,2)=0.0;
      }
      Matrix lambdas = iM*RightSide;
      
      weights.resize(iM.m);
      for (unsigned int i=0; i<weights.size(); ++i) {
         const Number x=lambdas(i,0);
         const Number y=lambdas(i,1);
         const Number z=lambdas(i,2);
         weights[i] = Point3(x,y,z);
      }
   }
   
   void execute(vector<Point3> &result) {
      
      assert(pPoints->size() == result.size());
      for (uint i=0; i<result.size(); ++i) {
         result[i] = evalf((*pPoints)[i]);
      }
   }
   
   Point3 evalf(const Point3 &p) {
      
      const uint &ncenters = centers.size();
      Number rx=0, ry=0, rz=0;
      /// radial component
      for (unsigned int i=0; i<ncenters; ++i) {
         const Point3 &P = centers[i];
         const Number &PHI = phi(p-P);
         rx += weights[i][0]*PHI;
         ry += weights[i][1]*PHI;
         rz += weights[i][2]*PHI;
      }
      
      /// polynomial component
      /// parte das colunas de lambdas sao os coeficientes do polinomio
      /// que corresponde ao componente x, y ou z
      for (unsigned int i=0; i<npolyterms; ++i) {
         const Number &valpoly = polynomial(p, i);
         rx += weights[ncenters+i][0]*valpoly;
         ry += weights[ncenters+i][1]*valpoly;
         rz += weights[ncenters+i][2]*valpoly;
      }
      return Point3(rx, ry, rz);
   }
   
   Number phi(const Vector3& v) {
      const Number &sqrLen = v*v;
      //return sqrLen*(sqrt(sqrLen)); // phi(v)=|v|^3
      return std::sqrt(sqrLen);   // phi(v)=|v|
   }
   
//private:
   vector<Point3>    weights;       // the vector of weights
};

#endif
